/*
*  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*  1. Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*
*  2. Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*
*  3. Neither the name of the copyright holder nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  abstract_planner_execution.h
*
*  authors:
*    Sebastian Pütz <spuetz@uni-osnabrueck.de>
*    Jorge Santos Simón <santos@magazino.eu>
*
*/

#ifndef MBF_ABSTRACT_NAV__ABSTRACT_PLANNER_EXECUTION_H_
#define MBF_ABSTRACT_NAV__ABSTRACT_PLANNER_EXECUTION_H_

#include <map>
#include <string>
#include <vector>

#include <geometry_msgs/PoseStamped.h>

#include <mbf_abstract_core/abstract_planner.h>
#include <mbf_utility/types.h>

#include <mbf_utility/navigation_utility.h>

#include "mbf_abstract_nav/abstract_execution_base.h"

namespace mbf_abstract_nav
{

/**
* @defgroup planner_execution Planner Execution Classes
* @brief The planner execution classes are derived from the AbstractPlannerExecution and extend the functionality.
*        The base planner execution code is located in the AbstractPlannerExecution.
*/

/**
* @brief The AbstractPlannerExecution class loads and binds the global planner plugin. It contains a thread running
*        the plugin in a cycle to plan and re-plan. An internal state is saved and will be pulled by the server, which
*        controls the global planner execution. Due to a state change it wakes up all threads connected to the
*        condition variable.
*
* @ingroup abstract_server planner_execution
*/
class AbstractPlannerExecution : public AbstractExecutionBase
{
public:

    //! shared pointer type to the @ref planner_execution "planner execution".
    typedef boost::shared_ptr<AbstractPlannerExecution> Ptr;

    /**
     * @brief Constructor
     * @param name Name of this execution
     * @param planner_ptr Pointer to the planner
     * @param config Initial configuration for this execution
     */
    AbstractPlannerExecution(const std::string &name, const mbf_abstract_core::AbstractPlanner::Ptr &planner_ptr,
                             const MoveBaseFlexConfig &config);

    /**
     * @brief Constructor
     * @param name Name of this execution
     * @param planner_ptr Pointer to the planner
     * @param tf_listener_ptr Shared Pointer to a tf-listener.
     * @param config Initial configuration for this execution
     */
    AbstractPlannerExecution(const std::string &name, const mbf_abstract_core::AbstractPlanner::Ptr &planner_ptr,
                             const TFPtr &tf_listener_ptr, const MoveBaseFlexConfig &config);

    /**
     * @brief Destructor
     */
    virtual ~AbstractPlannerExecution();

    /**
     * @brief Returns a new plan, if one is available.
     */
    std::vector<geometry_msgs::PoseStamped> getPlan() const;

    /**
     * @brief Returns the last time a valid plan was available.
     * @return time, the last valid plan was available.
     */
    ros::Time getLastValidPlanTime() const;

    /**
     * @brief Checks whether the patience was exceeded.
     * @return true, if the patience duration was exceeded.
     */
    bool isPatienceExceeded() const;

    /**
     * @brief Internal states
     */
    enum PlanningState
    {
        INITIALIZED,  ///< Planner initialized.  规划师初始化。
        STARTED,      ///< Planner started.      计划开始。
        PLANNING,     ///< Executing the plugin. 执行插件。
        FOUND_PLAN,   ///< Found a valid plan.   找到一个有效的计划。
        MAX_RETRIES,  ///< Exceeded the maximum number of retries without a valid command. 超过了最大重试次数。
        PAT_EXCEEDED, ///< Exceeded the patience time without a valid command. 超时
        NO_PLAN_FOUND,///< No plan has been found (MAX_RETRIES and PAT_EXCEEDED are 0).
        CANCELED,     ///< The planner has been canceled.
        STOPPED,      ///< The planner has been stopped.
        INTERNAL_ERROR///< An internal error occurred.
    };

    /**
     * @brief Returns the current internal state
     * @return the current internal state
     */
    PlanningState getState() const;

    /**
     * @brief Gets planning frequency
     */
    double getFrequency() const
    { return frequency_; };

    /**
     * @brief Gets computed costs
     * @return The costs of the computed path
     */
    double getCost() const;

    /**
     * @brief Cancel the planner execution. This calls the cancel method of the planner plugin.
     * This could be useful if the computation takes too much time, or if we are aborting the navigation.
     * @return true, if the planner plugin tries / tried to cancel the planning step.
     */
    virtual bool cancel();

    /**
     * @brief Sets a new goal pose for the planner execution
     * @param goal the new goal pose
     * @param tolerance tolerance to the goal for the planning
     */
    void setNewGoal(const geometry_msgs::PoseStamped &goal, double tolerance);

    /**
     * @brief Sets a new start pose for the planner execution
     * @param start new start pose
     */
    void setNewStart(const geometry_msgs::PoseStamped &start);

    /**
     * @brief Sets a new star and goal pose for the planner execution
     * @param start new start pose
     * @param goal new goal pose
     * @param tolerance tolerance to the new goal for the planning
     */
    void setNewStartAndGoal(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,
                            double tolerance);

    /**
     * @brief Starts the planner execution thread with the given parameters.
     * @param start start pose for the planning
     * @param goal goal pose for the planning
     * @param tolerance tolerance to the goal pose for the planning
     * @return true, if the planner thread has been started, false if the thread is already running.
     */
    bool start(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,
               double tolerance);

    /**
     * @brief Is called by the server thread to reconfigure the controller execution, if a user uses dynamic reconfigure
     *        to reconfigure the current state.
     * @param config MoveBaseFlexConfig object
     */
    void reconfigure(const MoveBaseFlexConfig &config);

protected:
    //! the local planer to calculate the robot trajectory
    mbf_abstract_core::AbstractPlanner::Ptr planner_;
    //! the name of the loaded planner plugin
    std::string plugin_name_;
    //! shared pointer to a common TransformListener
    const TFPtr tf_listener_ptr_;

    /**
     * @brief The main run method, a thread will execute this method. It contains the main planner execution loop.
     */
    virtual void run();

private:

    /**
     * @brief calls the planner plugin to make a plan from the start pose to the goal pose with the given tolerance,
     *        if a goal tolerance is enabled in the planner plugin.
     * @param start The start pose for planning
     * @param goal The goal pose for planning
     * @param tolerance The goal tolerance
     * @param plan The computed plan by the plugin
     * @param cost The computed costs for the corresponding plan
     * @param message An optional message which should correspond with the returned outcome
     * @return An outcome number, see also the action definition in the GetPath.action file
     */
    virtual uint32_t makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,
            double tolerance, std::vector<geometry_msgs::PoseStamped> &plan,
            double &cost, std::string &message);

    /**
     * @brief Sets the internal state, thread communication safe
     * @param state the current state
     * @param signalling set true to trigger the condition-variable for state-update
     */
    void setState(PlanningState state, bool signalling);

    //! mutex to handle safe thread communication for the current state
    mutable boost::mutex state_mtx_;
    //! mutex to handle safe thread communication for the plan and plan-costs
    mutable boost::mutex plan_mtx_;
    //! mutex to handle safe thread communication for the goal and start pose.
    mutable boost::mutex goal_start_mtx_;
    //! mutex to handle safe thread communication for the planning_ flag.
    mutable boost::mutex planning_mtx_;
    //! dynamic reconfigure mutex for a thread safe communication
    mutable boost::mutex configuration_mutex_;
    //! true, if a new goal pose has been set, until it is used.
    bool has_new_goal_;
    //! true, if a new start pose has been set, until it is used.
    bool has_new_start_;
    //! the last call start time, updated each cycle.
    ros::Time last_call_start_time_;
    //! the last time a valid plan has been computed.
    ros::Time last_valid_plan_time_;
    //! current global plan
    std::vector<geometry_msgs::PoseStamped> plan_;
    //! current global plan cost
    double cost_;
    //! the current start pose used for planning
    geometry_msgs::PoseStamped start_;
    //! the current goal pose used for planning
    geometry_msgs::PoseStamped goal_;
    //! optional goal tolerance, in meters
    double tolerance_;
    //! planning cycle frequency (used only when running full navigation; we store here for grouping parameters nicely)
    double frequency_;
    //! planning patience duration time
    ros::Duration patience_;
    //! planning max retries
    int max_retries_;
    //! main cycle variable of the execution loop
    bool planning_;
    //! robot frame used for computing the current robot pose
    std::string robot_frame_;
    //! the global frame in which the planner needs to plan
    std::string global_frame_;
    //! current internal state
    PlanningState state_;
};

} /* namespace mbf_abstract_nav */

#endif /* MBF_ABSTRACT_NAV__ABSTRACT_PLANNER_EXECUTION_H_ */
